#include "../include/SD.h"

const uint8_t sdcmd_tab[23][4] PROGMEM = 
{
  {SDC_GO_IDLE_STATE,		0x95,	SD_R1,	SD_NO_DATA},
  {SDC_SEND_OP_COND,		0xF9,	SD_R1,	SD_NO_DATA},
  {SDC_SEND_CSD,		0xAF,	SD_R1,	SD_MORE_DATA},
  {SDC_SEND_CID,		0x1B,	SD_R1,	SD_MORE_DATA},
  {SDC_STOP_TRANSMISSION,	0xC3,	SD_R1,	SD_NO_DATA},
  {SDC_SEND_STATUS,		0xAF,	SD_R2,	SD_NO_DATA},
  {SDC_SET_BLOCKLEN,		0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_READ_SINGLE_BLOCK,	0xFF,	SD_R1,	SD_MORE_DATA},
  {SDC_READ_MULTI_BLOCK,	0xFF,	SD_R1,	SD_MORE_DATA},
  {SDC_WRITE_SINGLE_BLOCK,	0xFF,	SD_R1,	SD_MORE_DATA},
  {SDC_WRITE_MULTI_BLOCK,	0xFF,	SD_R1,	SD_MORE_DATA},
  {SDC_TAG_SECTOR_START,	0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_TAG_SECTOR_END,		0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_UNTAG_SECTOR,		0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_TAG_ERASE_GRP_START,	0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_TAG_ERASE_GRP_END,	0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_UNTAG_ERASE_GRP,		0xFF,	SD_R1,	SD_NO_DATA},
  {SDC_ERASE,			0xDF,	SD_R1b,	SD_NO_DATA},
  {SDC_LOCK_UNLOCK,		0x89,	SD_R1b,	SD_NO_DATA},
  {SDC_APP_OP_COND,		0xE5,	SD_R1,	SD_NO_DATA},
  {SDC_APP_CMD,			0x73,	SD_R1,	SD_NO_DATA},
  {SDC_READ_OCR,		0x25,	SD_R3,	SD_NO_DATA},
  {SDC_CRC_ON_OFF,		0x25,	SD_R1,	SD_NO_DATA}
};

void sd_spi_init(void)
{
    SD_DDR |= (1<<SD_CS)|(1<<SD_MOSI)|(1<<SD_SCK); 	//CS, MOSI, SCK outputs
    SD_DDR &= ~(1<<SD_MISO);	//MISO input
    
    SD_PORT &= ~(1<<SD_MOSI) & ~(1<<SD_SCK);	//MOSI, SCK low
  
    SPCR = (0<<SPIE)|(1<<SPE)|(0<<DORD)|(1<<MSTR)|(0<<CPOL)|(0<<CPHA)|(1<<SPR1)|(0<<SPR0);
    SPSR = (1<<SPI2X);

    SD_SPI_DISABLE();
}	
void sd_spi_enable()
{
    SD_SPI_ENABLE();  
    SD_CS_ACTIVE();
}
void sd_spi_disable()
{
    SD_CS_INACTIVE();
    SD_SPI_DISABLE();
}
void sd_spi_write(uint8_t data)
{
//     DEBUG(fprintf_P(fRS,PSTR("sd_spi_write: data=  %x\n"),data));
    SD_SPI_DATA = data;
    SD_SPI_WAIT();
}

uint8_t sd_spi_read()
{
    uint8_t ret;
    SD_SPI_DATA = 0xff;
    SD_SPI_WAIT();
    ret = SD_SPI_DATA;
    return ret;
}

uint8_t sd_get_cmd(uint8_t sd_cmd)
{
    return pgm_read_byte(&sdcmd_tab[sd_cmd][0]);
}
uint8_t sd_get_crc(uint8_t sd_cmd)
{
    return pgm_read_byte(&sdcmd_tab[sd_cmd][1]);
}
uint8_t sd_get_resp(uint8_t sd_cmd)
{
    return pgm_read_byte(&sdcmd_tab[sd_cmd][2]);
}
uint8_t sd_get_tdata(uint8_t sd_cmd)
{
    return pgm_read_byte(&sdcmd_tab[sd_cmd][3]);
}

void 	sd_delay(uint8_t n)
{
    SD_CS_INACTIVE();
    while(n--)
      sd_spi_write(0xff);
}

uint8_t sd_send_cmd(sd_context_t * sdc,uint8_t cmd,uint32_t addr)
{
    uint8_t resp_type;
    uint8_t resp;
    uint8_t i;
    uint32_t timeout;
    resp_type = sd_get_resp(cmd);
    SD_CS_ACTIVE();
    sd_spi_write(sd_get_cmd(cmd));
    sd_spi_write(addr>>24);
    sd_spi_write(addr>>16);
    sd_spi_write(addr>>8);
    sd_spi_write(addr);
    sd_spi_write(sd_get_crc(cmd));
    timeout = SD_CMD_TIMEOUT;
    do
    {
	resp = sd_spi_read();
	timeout--;
    }while((resp&0x80)!=0 && timeout > 0);
//     DEBUG(fprintf_P(fRS,PSTR("sd_send_cmd: resp = %x\n"),resp));
    if(timeout == 0)
      return sderr_CardReadCmd_Timeout;
    switch(resp_type)
    {
      case SD_R1:
	sdc->R1 = resp;
	break;
      case SD_R1b:
	sdc->R1 = resp;
	break;
      case SD_R2:
	sdc->R1 = resp;
	sdc->R2 = sd_spi_read();
	break;
      case SD_R3:
	sdc->OCR[0] = resp;
	for(i=1;i<4;i++)
	  sdc->OCR[i] = sd_spi_read();
	sdc->R1 = sd_spi_read();
	break;
    }
    sd_delay(2);
    return 0;
}

uint8_t	sd_read_csd_cid(sd_context_t * sdc,uint8_t cmd)
{
    uint8_t retval;
    uint32_t timeout;
    uint8_t * ret;
    uint8_t i;
    timeout = SD_IDLE_WAIT_MAX;
    do
    {
      retval = sd_send_cmd(sdc,cmd,0x00);
      timeout--;
    }while(sdc->R1 != 0 && timeout > 0);
    if(retval)
      return sderr_ReadCSDCID;
    timeout = SD_READ_CSDCID_TIMEOUT;
    SD_CS_ACTIVE();
    do
    {
      retval = sd_spi_read();
      timeout--;
    }while(retval == SDC_FLOATING_BUS && timeout > 0);
    if(timeout==0)
      return sderr_ReadCSDCID_Timeout;
    if(retval != 0xFE)
      return sderr_ReadCSDCID_BadToken;
    if(cmd == sdcmd_SEND_CID)
      ret = (uint8_t*)&sdc->CID;
    else
      ret = (uint8_t*)&sdc->CSD;
    for(i=0;i<16;i++)
      (*ret++) = sd_spi_read();
    sd_delay(2);
    return 0;
}
uint32_t sd_size(sd_context_t * sdc)
{
    uint32_t cd_size,size;
    uint8_t mult;
    size=(sdc->CSD.field[9]&3);
    size = size<<10;
    size = size | (sdc->CSD.field[8]<<2);
    size = size | (sdc->CSD.field[7]>>6);
    mult= (sdc->CSD.field[6]&3)<<1;
    mult = mult | (sdc->CSD.field[5]<<7);
    cd_size = size;
    cd_size = (cd_size+1)<<(mult+2);
    cd_size = cd_size*512;
    return cd_size;
}
uint8_t sd_init(sd_context_t * sdc)
{
    uint32_t i;
    uint8_t retval;	                
    sd_spi_init();
    SD_SPI_ENABLE();
    SD_CS_INACTIVE();
    _delay_ms(100);
    sd_delay(10);
//     SD_CS_ACTIVE();
    _delay_ms(10); //10
    retval = sd_send_cmd(sdc,sdcmd_GO_IDLE_STATE,0x0);
    if(retval != 0)
    {
     goto init_error;
    }
//     DEBUG(fprintf_P(fRS,PSTR("GO_IDLE_STATE R1 = %x\n"),sdc->R1));
    if(sdc->R1 != 0x01)
    {
	retval = sderr_CardInit_NotInIdleState;
	goto init_error;
    }
    i = SD_IDLE_WAIT_MAX;
    do
    {
// 	retval = sd_send_cmd(sdc,sdcmd_APP_CMD,0x00);
	retval = sd_send_cmd(sdc,sdcmd_SEND_OP_COND,0x00);
	i--;
	if(retval)
	  i = 0;
    }while((sdc->R1 && 0x01) != 0 && i > 0);
//     DEBUG_PRINT("sdcmd_APP_CMD retval = %x, R1 = %x\n",retval,sdc->R1);
    //     DEBUG(fprintf_P(fRS,PSTR("SEND_OP_COND R1 = %x\n"),sdc->R1));  
    if(i==0)
    {
	retval = sderr_CardInit_Timeout;
	goto init_error;
    }
    i = SD_IDLE_WAIT_MAX;
    do
    {
	retval = sd_send_cmd(sdc,sdcmd_READ_OCR,0x0);
	i--;
	if(retval)
	  i=0;
    }while(((sdc->R1 && 0x01) != 0  && i > 0));
//     DEBUG(fprintf_P(fRS,PSTR("SEND_OP_COND R1 = %x\n"),sdc->R1));  
    if(i==0)
    {
	retval = sderr_CardInit_Timeout;
	goto init_error;
    }    
    if(retval)
      retval = sderr_CardInit_ReadOCR;
    retval = sd_read_cid(sdc);
    retval = sd_read_csd(sdc);
    if(retval)
      goto init_error;
init_error:
    SD_CS_INACTIVE();
    SD_SPI_DISABLE();
    return retval;
}

